/*
  BenderTestSuite
  Test suite to test functionality of Bender electronics
*/
#include <Bender.h>

int LEDBrightness = 255;

void setup() {
    // open the serial port:
  Serial.begin(115200);

  initBender();
  printInstructions();
}

void printInstructions(){
  Serial.println("Welcome to the Bender Test Suite");
  Serial.println("  h: Print this"); 
  Serial.println("  q: Turn Motor Clockwise");
  Serial.println("  w: Turn Motor Counter-Clockwise");
  Serial.println("  e: Stop the motor");
  Serial.println("  r: Decrease speed of motor by 10\%");
  Serial.println("  t: Increase speed of motor by 10\%");
  Serial.println("  a: Get values of hall effect encoder");
  Serial.println("  0-8: Enable Valve n, 8 turns them off");
  Serial.println("  z-,: Enable LED n, l turns them off");
}

void printEncoder(){
  int gs_state = digitalRead(HALL_GS);
  int a_zero_state = digitalRead(HALL1);
  int a_one_state = digitalRead(HALL2);
  int a_two_state = digitalRead(HALL3);
  Serial.print("GS: ");
  Serial.println(gs_state);
  Serial.print("A0: ");
  Serial.println(a_zero_state);
  Serial.print("A1: ");
  Serial.println(a_one_state);
  Serial.print("A2: ");
  Serial.println(a_two_state);
}

void loop() {
  // check for incoming serial data:
  if (Serial.available() > 0) {
    // read incoming serial data:
    char input = Serial.read();
    int led = 0;
    // Type the next ASCII value from what you received:
    switch(input){
      case 'h':
        printInstructions();
        break;
      case 'q':
        //CW Motor
        MotorCtrl(CW,motorSpeed);
        lastDirection = CW;
        break;
      case 'w':
        //CCW Motor
        MotorCtrl(CCW,motorSpeed);
        lastDirection = CCW;
        break;
      case 'e':
        //Stop Motor
        MotorCtrl(lastDirection, 0);
        break;
      case 'r':
        //Slow Motor
        motorSpeed = motorSpeed - 10;
        if(motorSpeed < 0){
          motorSpeed = 0;
        }
        MotorCtrl(lastDirection, motorSpeed);
        break;
      case 't':
        //Slow Motor
        motorSpeed = motorSpeed + 10;
        if(motorSpeed > 255){
          motorSpeed = 255;
        }
        MotorCtrl(lastDirection, motorSpeed);
        break;
      case 'a':
        printEncoder();
        break;
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
        openValve(input - '0');
        break;
      case '8':
        closeValves();
        break;
      case '\,': led++;
      case 'm':  led++;
      case 'n':  led++;
      case 'b':  led++;
      case 'v':  led++;
      case 'c':  led++;
      case 'x':  led++;
      case 'z':  
        turnOnLED(led, LEDBrightness);
        break;
      case 'l':
        turnOnLED(0, 255);
        break;
      case 'j':
        LEDBrightness = LEDBrightness - 10;
        if(LEDBrightness < 0){
          LEDBrightness = 0;
        }
        Serial.print("Decreasing brightness to ");
        Serial.println(LEDBrightness);
        analogWrite(LED_ENABLE, LEDBrightness);
        break;
      case 'k':
        LEDBrightness = LEDBrightness + 10;
        if(LEDBrightness > 255){
          LEDBrightness = 255;
        }
        Serial.print("Increasing brightness to ");
        Serial.println(LEDBrightness);
        analogWrite(LED_ENABLE, LEDBrightness);
        break;
      case '\n':
        break;
      default:
        Serial.println("Unrecognized command");
        printInstructions();
        break;
    }
  }
  if(buttonState == 1){
    Serial.println("Button Pressed!");
    buttonState = 0;
  }
  if(activeHallEffect >= 0){
    int gs_state = digitalRead(HALL_GS);
    if(gs_state == HIGH){
      Serial.print("Removed Hall from ");
      Serial.println(lastHallEffect);
    }
    else{
      Serial.print("Detected Hall ");
      Serial.println(activeHallEffect);
      lastHallEffect = activeHallEffect;
    }
    activeHallEffect = -1;
  }
}
